/*
The surface-physics project: http://code.google.com/p/surface-physics/
Adds real world physics based on the Bullet physics engine, to bases in the Orbiter space
flight simulation software.

Written by Abhijit Nandy
License : Apache License 2.0 

Bump_VesselWrapper.cpp
Bump_VesselWrapper management implementation.


*/


/* Class declaration */
#include "Bump_VesselWrapper.h"



DerivedRegister<Bump_VesselWrapper> Bump_VesselWrapper::reg("Bump_VesselWrapper");

//Orulex types
typedef union{
 double data[4];
 struct {double x,y,z,m;};
}VECTOR4;

extern double ORUGetElevByVec(VECTOR4 pos);
extern VECTOR4 v32v4(VECTOR3 v);


/**
 * Constructor for this derived class
 * Will be used if some attachment specific needs to be initialized comes up
 * Do NOT allocate memory here, let init() do that
 *
 */
Bump_VesselWrapper::Bump_VesselWrapper():
		WAIT_BE4_ATTACH_DELAY(2)
{

	//Raycast vehicle cant yet be deleted from physics world, though the VESSEL may be destroyed in
	//Orbiter, so deactivate it

	waitCount = 0;
	deleteIfInactive = false;



}


/**
 * Deletes attachment points &
 *
 * TODO: check if this works during scenario shutdown, vessel validity chk may help
 */
Bump_VesselWrapper::~Bump_VesselWrapper()
{
	sprintf(boutput, "Bump_VesselWrapper::~Bump_VesselWrapper: %s", strObjectName);
	oapiWriteLog(boutput);

	if(oapiIsVessel(bumpBase->hObjBulletBase) && oapiIsVessel(hObjThis) && physicsInited){
		sprintf(boutput, "Bump_VesselWrapper::~Bump_VesselWrapper: Valid handles");
		oapiWriteLog(boutput);

		detachOrbiterVESSEL();
	}




}


/**
 * Initialize a vessel wrapper
 */
int
Bump_VesselWrapper::init(Bump_BulletBase *bb, char *strObjName, OBJHANDLE hObj)
{
	int rv;
	
	bumpBase = bb;
	strcpy(strObjectName, strObjName);

/*	sprintf(boutput, "Bump_VesselWrapper::init: Initializing %s", strObjectName);
	oapiWriteLog(boutput);*/

	//If a oject ref has been passed, no need to search for the object
	if(hObj){
	/*	sprintf(boutput, "Bump_VesselWrapper::init: Object reference was passed for %s", strObjectName);
		oapiWriteLog(boutput);*/

		hObjThis = hObj;

		//VESSEL not yet attached but soon will be, hopefully!
		vWrappedVessel = (VESSEL3*)oapiGetVesselInterface(hObj);

	}
	else{
		//Create Orbiter object
		rv = searchOrbiterVESSEL();
		if(rv){
			sprintf(boutput, "Bump_VesselWrapper::init: ERROR : Searching Orbiter VESSEL failed %s", strObjectName);
			oapiWriteLog(boutput);
			return 1;
		}
	}

	//Set vessel status to landed to bypass ranging
/*			VESSELSTATUS2 vs;
			memset(&vs, 0, sizeof(VESSELSTATUS2));
			vs.version = 2;

			vWrappedVessel->GetStatusEx(&vs);
			vs.status = 4;
			vs.flag = 16 ;
			vWrappedVessel->DefSetStateEx(&vs);

			memset(&vs, 0, sizeof(VESSELSTATUS2));
			vs.version = 2;

			vWrappedVessel->GetStatusEx(&vs);

		DWORD d = vWrappedVessel->GetFlightStatus();*/


	//Do not set landed state just yet, only after attachment

	return 0;
}




/**
 * Checks if vessel radial diatance from Moon center is same as Orulex returned elevation
 * Attaches at proper direction if so using attachOrbiterVESSEL()
 *
 * Detaches if any thruster fired
 */
int
Bump_VesselWrapper::poststep(double SimT, double SimDT, double MJD)
{
	
	

	//sprintf(boutput, "Bump_VesselWrapper::poststep: The next sim step will advance the sim by %f secs", SimDT);
	//oapiWriteLog(boutput);

	if(physicsInited){
		
		//Check condition for detachment, total force's component in dir -G is > G
		VECTOR3 rdirThrust;

		if(vWrappedVessel->GetThrustVector(rdirThrust))
			detachOrbiterVESSEL();

	}
	else{
		//Determine is attachment is required by checking radial distance of vessel matches
		//Orulex elevation below vessel position or not
		double lng, lat, rad, elev;
		VECTOR3 gdirVesselRadiusVector;

		
		vWrappedVessel->GetGlobalPos(gposVesselPos);
		oapiGetGlobalPos(bumpBase->hObjRefBody, &gposMoon);

		gdirVesselRadiusVector = gposVesselPos - gposMoon; //Vector from Moon center to touchdown point in global frame
		elev = ORUGetElevByVec(v32v4(gdirVesselRadiusVector)); //Elevation measured from moon's center, no unit vector here
		
		vWrappedVessel->GetEquPos(lng, lat, rad);

		cogElev = vWrappedVessel->GetCOG_elev();
		rad -= cogElev;

		//sprintf(oapiDebugString(), "r:%f, maxte:%f, e:%f, waitCount:%d", rad, bumpBase->maxTerrainRadialDist, elev, waitCount);

		if(rad <= elev){
			waitCount--;
			if(waitCount < 0){
				attachOrbiterVESSEL();
				waitCount = WAIT_BE4_ATTACH_DELAY;
			}
		}
		else if(rad > bumpBase->maxTerrainRadialDist) //delete wrapper if vessel above terrain
			deleteMe = true;

			
	}

	return 0;
}


/**
 * Deallocates any memory, Bump_VesselWrapper::exit() will be called
 * by the containing Bump_BulletBase.
 * 
 * TODO: Check what has to be nulled 
 */
int
Bump_VesselWrapper::exit()
{
	

	//Base class members cleanup
	Bump_BaseObject::exit();

	return 0;

}


/**
 * Only resets the physics body. There is no need to re-attach
 *
 * Currently complete vehicle memory de-allocation is disabled due to m_actions[] in the
 * btDiscreteDynamicsWorld not being properly resized m_actions.size() fails in an assertion.
 *
 * This has to be reproduced in the Vehicle demo and asked in the Bullet forums.
 * Also test with re-spawn(reqd for Orulex) & deallocate memory options in Orbiter debugging options.
 *
 *
 * Meanwhile reset() causes the vehicle to turn back upright if toppled.
 *
 * TODO: Causes crash, fix destroyBulletRigidBody(),
 * Test in VehicleDemo
 */
int
Bump_VesselWrapper::searchOrbiterVESSEL()
{
	unsigned int i;
	unsigned int numSimVessels = oapiGetVesselCount();
	OBJHANDLE hObj;
	char strVesselName[MAX_STRLEN];


	//Search the Orbiter sim for this vessel
	for (i = 0; i<numSimVessels; i++) {

		//Assuming hObj will be non-NULL
		hObj = oapiGetVesselByIndex(i);
		oapiGetObjectName(hObj, strVesselName, MAX_STRLEN);

		sprintf(boutput, "Bump_VesselWrapper::init: %3d. Found : %s", i+1, strVesselName);
		oapiWriteLog(boutput);

		//Does the vessel name match this object's name ?
		if(strcmp(strVesselName, strObjectName) == 0){

			hObjThis = hObj;

			//VESSEL not yet attached but soon will be, hopefully!
			vWrappedVessel = (VESSEL3*)oapiGetVesselInterface(hObj);			

			return 0;
		}
	}



	return 1;
}


/**
 * Attach the VESSEL to the BulletBase in the Orbiter world
 *
 * TODO: not very efficient to search for the vessel each time
 *
 */
int
Bump_VesselWrapper::attachOrbiterVESSEL()
{
	char strAttID[8];;
	VECTOR3 gdirTDPointRadiusVector, gdirVesselRadiusVector, gposTouchdownPoint;

	VECTOR3 rdirUp, rdirForw, gposBB, gposElevTDPoint;
	VECTOR3 gdirXaxis, rdirXaxis;
	VECTOR3 pt1, pt2, pt3, v1, v2;
	double elev;

	//Convert Vessel's CURRENT global position into BulletBase relative position
	//gposVesselPos updated by poststep() which is caller
	gdirVesselRadiusVector = gposVesselPos - gposMoon;
	elev = ORUGetElevByVec(v32v4(gdirVesselRadiusVector));
	gposVesselPos = unit(gdirVesselRadiusVector) * (elev + cogElev); //this is actually only direction wrt Sun origin
	gposVesselPos += gposMoon;		//add moon gpos as vessel pos was found wrt Moon not BB
	bumpBase->bb->Global2Local(gposVesselPos , rposVesselPos); //convert point, no need to add BB


	vWrappedVessel->GetTouchdownPoints(pt1, pt2, pt3);



	//Get the 3 terrain points below the touchdown points
	vWrappedVessel->Local2Global(pt1, gposTouchdownPoint);	 //Convert from vessel frame to global
	gdirTDPointRadiusVector = gposTouchdownPoint - gposMoon; //Vector from Moon center to touchdown point in global frame
	elev = ORUGetElevByVec(v32v4(gdirTDPointRadiusVector));  //Elevation measured from moon's center, no unit vector here
	gposElevTDPoint = unit(gdirTDPointRadiusVector) * elev;  //Scale the unit vector in dir of TDpt by Orulex elev.
	gposElevTDPoint += gposMoon; 							 //translate above the moon from global origin
	bumpBase->bb->Global2Local(gposElevTDPoint, pt1);		 //convert global to base frame

	vWrappedVessel->Local2Global(pt2, gposTouchdownPoint);
	gdirTDPointRadiusVector = gposTouchdownPoint - gposMoon; //Vector from Moon center to touchdown point in global frame
	elev = ORUGetElevByVec(v32v4(gdirTDPointRadiusVector)); //Elevation measured from moon's center, no unit vector here
	gposElevTDPoint = unit(gdirTDPointRadiusVector) * elev;
	gposElevTDPoint += gposMoon; 							//translate above the moon from global origin
	bumpBase->bb->Global2Local(gposElevTDPoint, pt2);

	vWrappedVessel->Local2Global(pt3, gposTouchdownPoint);
	gdirTDPointRadiusVector = gposTouchdownPoint - gposMoon; //Vector from Moon center to touchdown point in global frame
	elev = ORUGetElevByVec(v32v4(gdirTDPointRadiusVector)); //Elevation measured from moon's center, no unit vector here
	gposElevTDPoint = unit(gdirTDPointRadiusVector) * elev;
	gposElevTDPoint += gposMoon; //translate above the moon from global origin
	bumpBase->bb->Global2Local(gposElevTDPoint, pt3);

	//sprintf(oapiDebugString(), "pt1:(%f,%f,%f)", V3ARGS(rposElevTDPoint));

	//Use the points to find the up vector and forward vector in base frame

	//2 arms of the triangle formed by the tdpts, meeting at pt2 in base frame
	v1 = pt1 - pt2;
	v2 = pt3 - pt2;

	//Normal to td plane is up vector
	rdirUp = crossp(v1,v2);
	rdirUp = unit(rdirUp); //this is in base frame

	//cross with x axis of vessel to get forward vector
	//TODO: Inline this as a function, conversion from vessel frame to base via global is common
	vWrappedVessel->GlobalRot(_V(1.f, 0.f, 0.f), gdirXaxis);//1st convert vessel x-axis to global frame

	bumpBase->bb->GetGlobalPos(gposBB);
	gdirXaxis += gposBB;  									//converting direction, add pos of target frame in global frame
	bumpBase->bb->Global2Local(gdirXaxis, rdirXaxis);		//2nd convert x-axis in global frame to base frame

	//Forward direction is cross of up vector in base frame & vessel's x-axis in base frame
	rdirForw = crossp(rdirXaxis, rdirUp);
	rdirForw = unit(rdirForw);

	//rdirUp = _V(0.f, 1.f, 0.f);
	//rdirForw = _V(0.f, 0.f, 1.f);

	//sprintf(oapiDebugString(), "f:(%f,%f,%f), u:(%f,%f,%f)", V3ARGS(rdirUp), V3ARGS(rdirForw));

	//sprintf(oapiDebugString(), "f:(%f,%f,%f)", V3ARGS(rposVesselPos));
	//Create an attachment point at the base, sep parent/child lists exist,  take next avail. pos.
	sprintf(strAttID, "BB%d", bumpBase->bb->AttachmentCount(FALSE));
	atBase = bumpBase->bb->CreateAttachment(false,
			rposVesselPos,
			rdirUp,
			rdirForw,
			 strAttID);



	//Create an attachment point for the VESSEL	, -1 temp. for debug.
	atVessel = vWrappedVessel->CreateAttachment(true, _V(0, 0, 0),
													 _V(0, -1, 0),
													 _V(0, 0, 1),
													 "BBA_CHILD");

	//Attachment points are valid ?
	if(atBase && atVessel){

		//Try attaching, child hObj is first parameter
		if(bumpBase->bb->AttachChild(hObjThis, atBase, atVessel)){

			//Success!, attached
		/*	sprintf(boutput, "Bump_VesselWrapper::attachOrbiterVESSEL: Attached %s to %s at attachment handle position %d",
								strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
			oapiWriteLog(boutput);*/

			//Set vessel status to landed to bypass ranging
			VESSELSTATUS2 vs;
			memset(&vs, 0, sizeof(VESSELSTATUS2));
			vs.version = 2;

			vWrappedVessel->GetStatusEx(&vs);
			vs.status = 1;
			vWrappedVessel->DefSetStateEx(&vs);

			physicsInited = true;

			//Return non-zero only if object was found and attached, anything else must return 1
			return 0;
		}
		else{ //Failure, could not attach
			sprintf(boutput,"Bump_ATV::attachOrbiterVESSEL: ERROR : Attach failed: p:%d c:%d &p:%p, &c:%p",
					bumpBase->bb->AttachmentCount(FALSE),
					vWrappedVessel->AttachmentCount(TRUE),
					atBase,
					atVessel);
			oapiWriteLog(boutput);

			//Should NULL all imp ptrs, to prevent use
			physicsInited = false;			
			atBase = NULL;
			atVessel = NULL;

			return 1; //attaching failure
		}



	}
	else{
		//One of the attachment handles could not be created
		sprintf(boutput,"Bump_ATV::attachOrbiterVESSEL: ERROR : Attachment handle creation failed: p:%d c:%d &p:%p, &c:%p",
									bumpBase->bb->AttachmentCount(FALSE),
									vWrappedVessel->AttachmentCount(TRUE),
									atBase,
									atVessel);
		oapiWriteLog(boutput);
	}

	physicsInited = false;	
	atBase = NULL;
	atVessel = NULL;

	//If we reach here, the vessel was not found
	return 2;
}


/**
 * Detach the VESSEL from the BulletBase in the Orbiter world
 *
 */
int
Bump_VesselWrapper::detachOrbiterVESSEL()
{
	bool rv1, rv2;

	//Attachment points are valid ?
	if(atBase && atVessel){

		//Try detaching child
		if(bumpBase->bb->DetachChild(atBase)){
			//Success!, detached
		/*	sprintf(boutput, "Bump_VesselWrapper::detachOrbiterVESSEL: Detached VESSEL %s from %s, "\
					"attachment count in base now %d",
					strObjectName, bumpBase->strBaseName, bumpBase->bb->AttachmentCount(FALSE));
			oapiWriteLog(boutput);*/

			//Delete the dynamically created attachment handles
			rv1 = bumpBase->bb->DelAttachment(atBase);
			rv2 = vWrappedVessel->DelAttachment(atVessel);

			if(!rv1 || !rv2){
				sprintf(boutput,"Bump_VesselWrapper::init: ERROR : Deletion failed for 1 attachment point(%d,%d): \
						p:%d c:%d &p:%p, &c:%p",
						rv1, rv2,
						bumpBase->bb->AttachmentCount(FALSE),
						vWrappedVessel->AttachmentCount(TRUE),
						atBase,
						atVessel);
				oapiWriteLog(boutput);

				return 1; //possible crash in next update if the below variables not NULLed
			}


			//These must be nulled after successful detach ONLY, so a crash occurs & the error fixed!
			physicsInited = false;
			atBase = NULL;
			atVessel = NULL;

			return 0;


		}
		else{ //Failure, could not detach
			sprintf(boutput,"Bump_VesselWrapper::init: ERROR : Detach failed: p:%d c:%d &p:%p, &c:%p",
					bumpBase->bb->AttachmentCount(FALSE),
					vWrappedVessel->AttachmentCount(TRUE),
					atBase,
					atVessel);
			oapiWriteLog(boutput);

			return 2;
		}

		oapiWriteLog(boutput);

	}
	else{
		//One of the attachment handles is not valid, cannot detach
		sprintf(boutput,"Bump_VesselWrapper::init: ERROR : Detachment failed due to invalid attachment handles: p:%d c:%d &p:%p, &c:%p",
									bumpBase->bb->AttachmentCount(FALSE),
									vWrappedVessel->AttachmentCount(TRUE),
									atBase,
									atVessel);
	}

	return 3;
}

